#include <Ping.h>
#include <Motor.h>

#define DEBUG

#ifdef DEBUG
#include <SoftwareSerial.h>
const int rx=10;
const int tx=9;
SoftwareSerial mySerial(rx, tx);
#endif

const int trig = 1;
const int echo = 0;

int right_Count = 0;
int right_turn_count = 0;

Motor motor_Driver(7,8,5,4,6,3);

UltrasonicSensor ultra(echo,trig,50);

void turn_right_90(){
    #ifdef DEBUG
      mySerial.println("Making a 90 degree right!");
  #endif
   motor_Driver.drive_forward();
   delay(100);
   motor_Driver.turn_right();
   delay(500);
   motor_Driver.drive_forward();
   delay(75);
}

void setup() {
#ifdef DEBUG  
  pinMode(rx,INPUT);
  pinMode(tx,OUTPUT);
  mySerial.begin(9600);
#endif
}


void loop() {
    right_Count = 0;
    unsigned int temp = ultra.getDistance();
    #ifdef DEBUG
        mySerial.print("Ping: ");
        mySerial.println(temp );
    #endif
    
    
/*
I started this section with if else loops. The turns were very wide.
So i changed it to while but i have not tested it yet.
I changed the delay at the end from 5 to 500 ms so that may help also.
*/

    if(temp == 0 && right_turn_count > 4){
        turn_right_90();
        temp = 14;
        right_turn_count = 0;
    }
    while(temp > 15 && temp != 0){//I want to make a sharper right
        right_Count++;
        #ifdef DEBUG
            mySerial.print("Right Count: ");
            mySerial.println(right_Count);
        #endif
        if(right_Count < 5){
            #ifdef DEBUG
                mySerial.println("Turning right!");
            #endif
            motor_Driver.turn_right();
            delay(30);
            motor_Driver.drive_forward();
            delay(50);
            motor_Driver.stop();
            delay(1);
            temp = ultra.getDistance();
            #ifdef DEBUG
                mySerial.print("Ping: ");
                mySerial.println(temp );
            #endif
        }else{
            temp = 14;            
            #ifdef DEBUG
                mySerial.print("EXITING THE LOOP!!!!");
            #endif
        }        
    }
    while(temp < 10 && temp != 0){//I want to make a sharper left
        #ifdef DEBUG
            mySerial.println("Turning left!");
        #endif
        motor_Driver.turn_left();
        delay(30);
        motor_Driver.drive_forward();
        delay(50);
        motor_Driver.stop();
        delay(1);
        temp = ultra.getDistance();
        #ifdef DEBUG
            mySerial.print("Ping: ");
            mySerial.println(temp );
        #endif 
    }

    #ifdef DEBUG
        mySerial.println("Driving forward!");/*base case is driving forward*/
    #endif
    right_turn_count++;
    motor_Driver.drive_forward();
        delay(50);

  /*
  mySerial.print("Ping distance: ");
  mySerial.println(ultra.getDistance());
  
  mySerial.println("Driving Forward then pausing for 5 sec.");
  motor_Driver.drive_forward();
  delay(10000);
  
  mySerial.println("Turning right then pausing for 5 sec.");
  motor_Driver.turn_right();
  delay(10000);
  
  mySerial.println("Turning Left then pausing for 5 sec.");
  motor_Driver.turn_left();
  delay(10000);
  
  mySerial.println("Backards then pausing for 5 sec.");
  motor_Driver.drive_backward();
  delay(10000);
  
  mySerial.println("STOP then pausing for 5 sec.");
  motor_Driver.stop();
  delay(10000);
  */
}

